/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.PID.subsystems;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SendablePIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Timer;
import java.util.TimerTask;
import ticks.PID.RobotMap;
import ticks.PID.commands.CommandBase;

/**
 *
 * @author Jared
 */
public class Drive {

    private SpeedController left_drive;
    private SpeedController right_drive;
    private RobotDrive drive;
    private Gyro gyro;
    private SendablePIDController turn_controller;
    private Timer driveTimer;
    private Joystick driveStick;
    private DriveTask m_driveTask;
    private double loopTime = 50;
    private double rotateGain;
    private double x, y;
    private boolean manualMode = false;
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public Drive() {
        left_drive = new Victor(RobotMap.Drive.left_motor_pwm);
        right_drive = new Victor(RobotMap.Drive.right_motor_pwm);
        drive = new RobotDrive(left_drive, right_drive);
        gyro = new Gyro(RobotMap.Drive.gyro_in);
        m_driveTask = new DriveTask();
        driveTimer = new Timer();
        driveTimer.schedule(m_driveTask, 0L, (long) Double.doubleToLongBits(loopTime));
        turn_controller = new SendablePIDController(0, 0, 0, gyro, m_driveTask);
        turn_controller.setContinuous(false);
        turn_controller.enable();
        SmartDashboard.putData("Turn Controller", turn_controller);
        SmartDashboard.putDouble("RotateGain", rotateGain);
        driveStick = CommandBase.oi.getDriveStick();
        gyro.reset();
    }

    public void autoDrive(double x, double y) {
        this.x = x;
        this.y = y;
    }

    public void disableGyroControl() {
        manualMode = true;
    }

    private class DriveTask extends TimerTask implements PIDOutput {

        private double pidOutput;
        private double joystick_x;
        private double joystick_y;
        private double angle_setpoint = 0;
        private double m_rotateGain;

        public void run() {
            if (DriverStation.getInstance().isOperatorControl()) {
                if (manualMode) {
                    drive.arcadeDrive(driveStick);
                } else {
                    joystick_x = driveStick.getRawAxis(1);
                    joystick_y = driveStick.getRawAxis(2);
                    m_rotateGain = SmartDashboard.getDouble("RotateGain", 1);
                    angle_setpoint = angle_setpoint + (joystick_x * m_rotateGain);
                    turn_controller.setSetpoint(angle_setpoint);
                    drive.arcadeDrive(joystick_y, pidOutput);
                }
            } else {
                joystick_x = x;
                joystick_y = y;
                m_rotateGain = SmartDashboard.getDouble("RotateGain", 1);
                angle_setpoint = angle_setpoint + (joystick_x * m_rotateGain);
                turn_controller.setSetpoint(angle_setpoint);
                drive.arcadeDrive(joystick_y, pidOutput);
            }
        }

        public synchronized void pidWrite(double output) {
            pidOutput = output;
        }
    }
}
